/*
 * tracker.cpp
 *
 *  Created on: 13.06.2011
 *      Author: nick
 */

#include "tracker.h"

// init instance of the tracker to 0
CTracker* CTracker::m_instance = 0;

// Singleton object access function
CTracker* CTracker::instance()
{
	 if( m_instance == 0 )
	    m_instance = new CTracker();
	  return m_instance;
}

//CTOR
CTracker::CTracker() : m_unprocessedMarks(), m_trackedMarks(), m_nOfMarks(0), m_results(),
		m_currentFrame(), m_latestFrame()
{	m_blobImg = cvCreateImage(cvSize(640, 480), 8, 1);}  //TODO: defines fuer picsize verwenden

//! DTOR
CTracker::~CTracker()
{
	Mat *del;
	CMark *delMark;

	//delete all results
	while(!m_results.empty()) {
		del = m_results.front();
		m_results.pop_front();
		delete del;
	}
	//delete all unprocessed marks
	while(!m_unprocessedMarks.empty()){
		delMark = m_unprocessedMarks.front();
		m_unprocessedMarks.pop_front();
		delete delMark;
	}
	//delete all tracked marks
	while(!m_trackedMarks.empty()){
		delMark = m_trackedMarks.front();
		m_trackedMarks.pop_front();
		delete delMark;
	}
}

// performs a blob analysis on the overloaded mat. The results are written to the unprocessedMarks list
void CTracker::blobAnalysis(cv::Mat &mat)
{
  CBlobResult blobs, filteredBlobs;
	CBlob* blob;
	//blobanalyse
	IplImage redContentImg = mat.operator IplImage();
	blobs = CBlobResult( &redContentImg, NULL, 10, true );

	//Blobs gr��er als 4000 und kleiner als 200 entfernen
	blobs.Filter( filteredBlobs, B_EXCLUDE, CBlobGetArea(), B_GREATER, 7000);
	filteredBlobs.Filter(filteredBlobs, B_EXCLUDE, CBlobGetArea(), B_LESS, 200);

	cvFillImage(m_blobImg, 0 );
	//�ber alle gefundenen blobs iterieren
	int i;
	for(i=0; i<filteredBlobs.GetNumBlobs(); i++) {
		blob = filteredBlobs.GetBlob(i);
		CMark *newMark = new CMark(blob);
		m_unprocessedMarks.push_front( newMark );
		blob->FillBlob( m_blobImg , CV_RGB(0,255,100));
	}

#ifdef SHOW_IMGS
	cvNamedWindow( "BlobImg", CV_WINDOW_AUTOSIZE );
	cvShowImage( "BlobImg", m_blobImg );
#endif
}

// filter the crap out of the the unprocessed marks
void CTracker::filterUnprocessedMarks()
{
	bool deleteThisMark = false;
	list<CMark*> toBeDeleted;
	list<CMark*>::const_iterator iUnprocessedMark;

	for( iUnprocessedMark = m_unprocessedMarks.begin(); iUnprocessedMark != m_unprocessedMarks.end() ; iUnprocessedMark++ )
	{
		if( (*iUnprocessedMark)->roundness() > ROUNDNESS_UPPER_LIMIT )
			deleteThisMark = true;

		//TODO: weitere harte filterkriterien? position zu weit in der mitte?

		if(deleteThisMark)
			toBeDeleted.push_front( (*iUnprocessedMark) );
	}


	//delete marks
	CMark *del;
	while( !toBeDeleted.empty() ) {
		del = toBeDeleted.front();
		m_unprocessedMarks.remove(del);
		toBeDeleted.pop_front();
		delete del;
	}
}

// extract all relevant tracking informations from a picture. returns true if at least one mark has been found
bool CTracker::processFrame( Mat &mat)
{
	Mat outYC, redContent, out, color_blue, color_green, color_red, color_diff ;
	vector<Mat> YCcolors;
	mat.copyTo(out);
	mat.copyTo(m_currentFrame);

#ifdef FLOORMASK
	applyFloorMask(mat);
#endif

#ifdef YCRCB
#ifdef ROS_BUILD
	cvtColor(mat, outYC, CV_BGR2YCrCb);
	split(outYC, YCcolors);
	threshold(YCcolors[2], redContent, RED_SEGMENTATION_THRESH , 255, CV_THRESH_BINARY);
#else
	//TODO: broken - dont use RGB atm
	cvtColor(mat, outYC, CV_BGR2YCrCb);
	split(outYC, YCcolors);
	threshold(YCcolors[1], redContent, RED_SEGMENTATION_THRESH , 255, CV_THRESH_BINARY);
#endif
#else
	split(mat,YCcolors);
	subtract(color_red, color_green, color_diff);
	subtract(color_diff, color_blue, color_diff);
	redContent = color_diff;
#endif

	blobAnalysis(redContent);
	filterUnprocessedMarks();
	incAllUpdateCounters();
	processMarks();
	drawDataMat(out);
	drawHUD(out);
	m_latestFrame = out;


#ifdef SHOW_IMGS
	namedWindow("INPUT", 1);
	cvMoveWindow("INPUT", 0, 100);
	imshow("INPUT", mat);

	namedWindow("Rot", 1);
	cvMoveWindow("Rot", 680, 200);
	imshow("Rot", redContent);

	namedWindow("Output", 1);
	cvMoveWindow("Output", 0, 200);
	imshow("Output", out);
#endif

	if(!m_results.empty())
		return true;
	return false;
}

// if a mark has been found this function returns a pointer to it (CHECK FOR NULLPOINTER!)
Mat* CTracker::getTrackedMark()
{
	Mat* result;
	while( !m_results.empty() )
	{
		result = m_results.front();
		m_results.pop_front();
		return result;
	}
	return NULL;
}

// returns the latest processed frame containing all informations
Mat& CTracker::latestProcessedFrame()
{	return m_latestFrame; }

// write a string into a frame  (top left)
void CTracker::writeStringToFrame(Mat& frame, string str )
{
	int boxsize = 30;
	//black box at the top:
	rectangle(frame, Point2i(0,boxsize/2), Point2i(640, boxsize/2), Scalar(0,0,0), boxsize, 1,0 );
	putText( frame , str, Point2i(5,boxsize/2) , 1, 1, Scalar(255,255,255), 1 , 8 , false );
}

// draws the data of the tracked marks into the overloaded mat
void CTracker::drawDataMat(cv::Mat &mat) const
{
	list<CMark*>::const_iterator iTrackedMark;
	for( iTrackedMark = m_trackedMarks.begin(); iTrackedMark != m_trackedMarks.end() ; iTrackedMark++ ) {
		(*iTrackedMark)->drawDataToMat(mat, false);
	}
}

// draws the data of the unprocessed marks into the overloaded mat
void CTracker::drawDebugDataMat(cv::Mat &mat) const
{
	list<CMark*>::const_iterator iUnprocessedMark;
	for( iUnprocessedMark = m_unprocessedMarks.begin(); iUnprocessedMark != m_unprocessedMarks.end() ; iUnprocessedMark++ ) {
		(*iUnprocessedMark)->drawDataToMat(mat, false);
	}
}

// draw some useful informations into the overloaded mat
void CTracker::drawHUD(cv::Mat &mat) const
{
	int boxsize = 40;
	//black box at the top:
	rectangle(mat, Point2i(0,boxsize/2), Point2i(640, boxsize/2), Scalar(0,0,0), boxsize, 1,0 );
	//prepare string:
	string text =
				"UPM: " + intToString(m_unprocessedMarks.size()) + "  "	 + //unprocessed marks
				"TM: " + intToString(m_trackedMarks.size()) + "  " + 			//tracked marks
				"Results: " + intToString( m_nOfMarks );


	putText( mat , text, Point2i(5,boxsize/2) , 1, 1, Scalar(255,255,255), 1 , 8 , false );
}

// applys the floormask to a picture. All irrelevant areas are blacked out
void CTracker::applyFloorMask(Mat &frame)
{
	//TODO: ordentlich als maske machen und nicht fuer jeden frame neu berechnen
	Point polygon0[] = {
			Point(frame.cols, frame.rows),
			Point(0, frame.rows),
			Point(0, frame.rows - DBOTTOM1),
			Point(frame.cols, frame.rows - DBOTTOM1)
	};
	Point polygon1[] = {
			Point(0, frame.rows - DBOTTOM1),
			Point(DLEFT, frame.rows - DBOTTOM2),
			Point(frame.cols - DRIGHT, frame.rows - DBOTTOM2),
			Point(frame.cols, frame.rows - DBOTTOM1),
	};
	Point polygon2[] = {
			Point(0,0),
			Point(frame.cols, 0),
			Point(frame.cols, DTOP1),
			Point(0, DTOP1)
	};
	Point polygon3[] = {
			Point(0, DTOP1),
			Point(DLEFT, DTOP2),
			Point(frame.cols - DRIGHT, DTOP2),
			Point(frame.cols, DTOP1),
	};
	Point polygon4[] = {
			Point(220, 0),
			Point(frame.cols - 220, 0),
			Point(frame.cols - 220, frame.rows),
			Point(220, frame.rows),
	};

	fillConvexPoly(frame, polygon0, 4, Scalar(0, 0, 0));
	fillConvexPoly(frame, polygon1, 4, Scalar(0, 0, 0));
	fillConvexPoly(frame, polygon2, 4, Scalar(0, 0, 0));
	fillConvexPoly(frame, polygon3, 4, Scalar(0, 0, 0));
	fillConvexPoly(frame, polygon4, 4, Scalar(0, 0, 0));
}

// creates a picture of a tracked mark and adds it to the result list
void CTracker::createResultPic(CMark* mark)
{
	mark->drawDataToLatestFrame();
	Mat *result = new Mat(mark->lastFrame());
	m_results.push_back(result);
}

// increments the updateCounters of all tracked marks
void CTracker::incAllUpdateCounters() const
{
	list<CMark*>::const_iterator iTrackedMark;
	for( iTrackedMark = m_trackedMarks.begin(); iTrackedMark != m_trackedMarks.end() ; iTrackedMark++ ) {
		(*iTrackedMark)->incUpdateCounter();
	}
}

// try to merge the new marks with the ones already known
void CTracker::processMarks()
{
	list<CMark*> toBeDeleted;
	list<CMark*> toBeRmvFTrackedList;
	list<CMark*> toBeRmvFUnprocessedList;

	int simi;
	bool emptyList, addMarkToTrackedList = false;
	list<CMark*>::const_iterator iTrackedMark;
	list<CMark*>::const_iterator iToBeDeletedMark;
	list<CMark*>::const_iterator iNewMark;

	//the list is empty in the first iteration -> add all unprocessed marks to the being-tracked list
	if(m_trackedMarks.empty())
		emptyList = true;

	for( iNewMark = m_unprocessedMarks.begin(); iNewMark != m_unprocessedMarks.end() ; iNewMark++ ) {
		addMarkToTrackedList = false;

		//compare the new mark with all tracked marks
		for( iTrackedMark = m_trackedMarks.begin(); iTrackedMark != m_trackedMarks.end() ; iTrackedMark++ ) {

			//compare the two marks
			simi = (*iNewMark)->equals(*iTrackedMark);

			//if they are the same - update the tracked mark with the new data
			if(simi  >= SIMILARITY_LIMIT) {
				(*iTrackedMark)->updateData(*iNewMark, /* m_latestFrame */ m_currentFrame);
				toBeDeleted.push_front( *iNewMark );
				toBeRmvFUnprocessedList.push_front( *iNewMark );
			}

			//if not, set the flag to add the new mark to the being-tracked list
			else
				addMarkToTrackedList = true;

		}//end foreach tracked mark

		//if the being-tracked list is empty, add all unprocessed marks to it
		if(emptyList){
			m_trackedMarks.push_front( *iNewMark );
			toBeRmvFUnprocessedList.push_front( *iNewMark );
		}
		else if(addMarkToTrackedList){
			m_trackedMarks.push_front( *iNewMark );
			toBeRmvFUnprocessedList.push_front( *iNewMark );
		}
	}//end foreach unprocessed mark

	for( iTrackedMark = m_trackedMarks.begin(); iTrackedMark != m_trackedMarks.end() ; iTrackedMark++ ) {

		/* if the mark has been updated once in the last UPDATES_ALLOWED_TO_MISS frames
		 *  AND it's last position has been at the border */
		if( ( (*iTrackedMark)->updateCounter() > UPDATES_ALLOWED_TO_MISS  )
					&& ( (*iTrackedMark)->lastSeenOnBorder())
					&& (	(*iTrackedMark)->tracked()		)
		)
		{
			createResultPic(*iTrackedMark);
			m_nOfMarks++;
			toBeDeleted.remove( *iTrackedMark );
			toBeDeleted.push_front( *iTrackedMark );
		}
		else if(  ( (*iTrackedMark)->updateCounter() > UPDATES_ALLOWED_TO_MISS  ) )
		{
			toBeDeleted.remove( *iTrackedMark );
			toBeDeleted.push_front( *iTrackedMark );
		}

	}

	//remove marks from the being-tracked list
	while( !toBeRmvFTrackedList.empty() ){
		m_trackedMarks.remove(toBeRmvFTrackedList.front());
		toBeRmvFTrackedList.pop_front();
	}
	//remove marks from the unprocessed list
	while( !toBeRmvFUnprocessedList.empty() ){
		m_unprocessedMarks.remove(toBeRmvFUnprocessedList.front());
		toBeRmvFUnprocessedList.pop_front();
	}
	//delete marks
	CMark *del;
	while( !toBeDeleted.empty() ) {
		del = toBeDeleted.front();
		m_trackedMarks.remove(del);
		m_unprocessedMarks.remove(del);
		toBeDeleted.pop_front();
		delete del;
	}

}//processMarks()
